#pragma once
#ifndef _OPTIMISERT_H_
#define _OPTIMISERT_H_
#include "complement.h"

#include <opencv2/opencv.hpp>
//#include "global.h"




struct MarkerPixel
{
	Point3f m_3d;
	uchar i;
};



class Camera
{
public:
	Camera(const Mat &_R, const Mat &_t) :R(_R), t(_t) {};
    Camera(){};
	~Camera() {};
	Camera(const Camera&b);
	void applyTranform(const double *p);
	Matx33d R;
	Matx31d t;
	template<typename T>
	void TransformToCameraPlane(const Point3_<T> &s, Point3_<T>&d);
	template<typename T>
	void TransformToCameraPlane(const vector<Point3_<T>> &src, vector<Point3_<T>> &dst);
};

class IntrinsicMatrix
{
public:
	IntrinsicMatrix(const Matx33d&_K) :K(_K), fx(K(0)), fy(K(4)), cx(K(2)), cy(K(5)) {};
	~IntrinsicMatrix() {};
	IntrinsicMatrix(const IntrinsicMatrix &b);
	IntrinsicMatrix applyScale(double scale);
	Matx33d K;
	double fx, fy, cx, cy;
	template<typename T,typename T2>
	void projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &dst);
	template<typename T,typename T2>
	void projectToImage(const Point3_<T> &s, Point_<T2> &d);
private:

};


class OptimiseRt
{
public:
    OptimiseRt(){};
    ~OptimiseRt(){};
    void optimise(const Mat&nowImg,const IntrinsicMatrix &K,Camera& C,const vector<Point3f>&pts3d,const vector<Point2f> &pts2d,const Mat&gx,const Mat&gy,double w[3])const;
    vector<MarkerPixel> M;
private:
    
};

template<typename T,typename T2>
void IntrinsicMatrix::projectToImage(const vector<Point3_<T>> &s, vector<Point_<T2>> &d)
{
	d.resize(s.size());
	for (size_t i = 0; i < s.size(); i++)
	{
		projectToImage(s[i], d[i]);
	}
}

template<typename T,typename T2>
inline void IntrinsicMatrix::projectToImage(const Point3_<T> &s, Point_<T2> &d)
{
    d.x = s.x * fx / s.z+cx;
    d.y = s.y * fy / s.z+cy;
}

template<typename T>
void Camera::TransformToCameraPlane(const Point3_<T> &s, Point3_<T>&d)
{
	d.x = s.x*R(0,0) + s.y*R(1,0) + s.z*R(2,0) + t(0);
	d.y = s.x*R(0,1) + s.y*R(1,1) + s.z*R(2,1) + t(1);
	d.z = s.x*R(0,2) + s.y*R(1,2) + s.z*R(2,2) + t(2);
    
    
//    const auto &r = (const double*)R.data;
//    const auto &tt = (const double*)t.data;
//    d.x = s.x*r[0] + s.y*r[1] + s.z*r[2] + tt[0];
//    d.y = s.x*r[3] + s.y*r[4] + s.z*r[5] + tt[1];
//    d.z = s.x*r[6] + s.y*r[7] + s.z*r[8] + tt[2];
}

template<typename T>
void Camera::TransformToCameraPlane(const vector<Point3_<T>> &src, vector<Point3_<T>> &dst)
{
//	CV_Assert(R.type() == CV_64F&&t.type() == CV_64F);
	dst.clear();
	dst.resize(src.size());

	for (size_t i = 0; i < src.size(); i++)
	{
		const auto &s = src[i];
		auto &d = dst[i];
		TransformToCameraPlane(s, d);
	}
}

void _jacf(double *p, double *j, int m, int n, void *adata);
void _func(double *p, double *hx, int m, int n, void *adata);
Mat functionF(const Mat&f);
Mat functionl(const Mat&delta, const Mat &J, const Mat &f);
Mat functionL(const Mat&l);
void updateRt(const double p[8],Mat &R,Mat &t);
Mat computeStep(const Mat&A, const double mu, const Mat &G);
void updateStep(const Mat&delta, double pp[8]);
double computeInitalDamping(const Mat&JtJ);
Mat computeEnergyLM(const vector<MarkerPixel> &M, const Mat &I, IntrinsicMatrix&K, Camera&C, const double * p, const vector<Point3f> &kp3, const vector<Point2f> &kp2, double w1, double w2,double scale);
Mat constructA(const Mat &J);
Mat constructG(const Mat&J, const Mat&f);
void computeGradient(const Mat & _src, Mat & gx, Mat & gy);
void computeJacobian(const vector<MarkerPixel> &M, const Mat &I, const Mat&gx, const Mat&gy, IntrinsicMatrix&K, Camera &C, const double*p, const vector<Point3f> &kp3, const vector<Point2f> &kp2, double w1, double w2, double scale,Mat &J);
void verifyJacobian(const vector<MarkerPixel> &M, const Mat &I, IntrinsicMatrix&K, Camera&C, float * p, const vector<Point3f> &kp3, const vector<Point2f> &kp2, double w1, double w2,double scale, const Mat &J);
#endif